/*  ----------------------------------------------------------------------------
 *  Arello Mobile
 *  Fast 3d Engine
 *  ----------------------------------------------------------------------------
 * Except where otherwise noted, this work is licensed under
 * http://creativecommons.org/licenses/by/3.0
 */

#ifndef __MATRIX4_H__
#define __MATRIX4_H__

#include "Matrix.h"
#include "Vector3.h"

#ifdef __APPLE__
#include <TargetConditionals.h>
#if (TARGET_IPHONE_SIMULATOR == 0) && (TARGET_OS_IPHONE == 1)
#include "iphone/matrix_impl.h"
#endif
#endif

namespace liba
{
namespace math
{

template<typename T>
class Matrix4: public MatrixSquare<T,4>
{
	typedef MatrixSquare<T,4> MatrixBase;
public:
	Matrix4()
	{}
	Matrix4( const MatrixSquare<T,4> & mat )
		:	MatrixSquare<T,4>( mat )
	{}
	
	static Matrix4 translate( const Vector3<T> & v )
	{
		Matrix4 tra = MatrixBase::identity();
		tra.rc(0,3) = v.x;
		tra.rc(1,3) = v.y;
		tra.rc(2,3) = v.z;
		return tra;
	}
	static Matrix4 scale( const Vector3<T> & v )
	{
		Matrix4 sca = MatrixBase::identity();
		sca.rc(0,0) = v.x;
		sca.rc(1,1) = v.y;
		sca.rc(2,2) = v.z;
		return sca;
	}
	static Matrix4 invert( Matrix4 m )
	{
		Matrix4 res = MatrixBase::identity();
		for( int i = 0; i < 4; ++i )
		{
			int ma = i;
			for( int j = ma + 1; j < 4; ++j )
				if( fabs( m.rc(j, i) ) > fabs( m.rc(ma, i) ) )
					ma = j;
			if( ma != i )
			for( int nx = 0; nx < 4; ++nx )
			{
				T tmp = m.rc( i, nx );
				m.rc( i, nx ) = m.rc( ma, nx );
				m.rc( ma, nx ) = tmp;
				tmp = res.rc( i, nx );
				res.rc( i, nx ) = res.rc( ma, nx );
				res.rc( ma, nx ) = tmp;
			}
			for( int ny = 0; ny < 4; ++ny )
				if( ny == i )
				{
					T mul = 1 / m.rc(i, i);
					for( int nx = 0; nx < 4; ++nx )
					{
						m.rc( ny, nx ) *= mul;
						res.rc( ny, nx ) *= mul;
					}
				}
				else
				{
					T mul = m.rc( ny, i ) / m.rc(i, i);
					for( int nx = 0; nx < 4; ++nx )
					{
						m.rc( ny, nx ) -= mul * m.rc( i, nx );
						res.rc( ny, nx ) -= mul * res.rc( i, nx );
					}
				}
		}
		return res;
	}
	Vector3<T> shift()const
	{
		return Vector3<T>( MatrixBase::rc(0,3), MatrixBase::rc(1,3), MatrixBase::rc(2,3) );
	}
	
	static Matrix4 rotate( const Vector3<T> & angles )
	{
		T cz(cos( angles.z ));
		T sz(sin( angles.z ));
		T cy(cos( angles.y ));
		T sy(sin( angles.y ));
		T cx(cos( angles.x ));
		T sx(sin( angles.x ));
		Matrix4 mat = MatrixBase::identity();
		mat.rc(0,0) = cz*cy;
		mat.rc(1,0) = sz*cy;
		mat.rc(2,0) = sy;
		mat.rc(0,1) = -cz*sx*sy - sz*cx;
		mat.rc(1,1) = -sz*sx*sy + cz*cx;
		mat.rc(2,1) = sx*cy;
		mat.rc(0,2) = -cz*cx*sy + sz*sx;
		mat.rc(1,2) = -sz*cx*sy - cz*sx;
		mat.rc(2,2) = cx*cy;
		return mat;
	}

	static Matrix4 rotate( const Vector3<T> & axe, T angle )
	{
		Matrix4 mat = MatrixBase::identity();

		T c = (T)cos( angle );
		T s = (T)sin( angle );

		mat.rc(0,0) = axe.x*axe.x		+(1-axe.x*axe.x)*c;
		mat.rc(0,1) = axe.x*axe.y*(1-c)	+axe.z*s;
		mat.rc(0,2) = axe.x*axe.z*(1-c)	-axe.y*s;

		mat.rc(1,0) = axe.x*axe.y*(1-c)	-axe.z*s;
		mat.rc(1,1) = axe.y*axe.y			+(1-axe.y*axe.y)*c;
		mat.rc(1,2) = axe.y*axe.z*(1-c)	+axe.x*s;

		mat.rc(2,0) = axe.x*axe.z*(1-c)	+axe.y*s;
		mat.rc(2,1) = axe.y*axe.z*(1-c)	-axe.x*s;
		mat.rc(2,2) = axe.z*axe.z			+(1-axe.z*axe.z)*c;
		return mat;
	}

	static Vector3<T> factorization( const Matrix4 & mat )
	{
		T angle_x, angle_y, angle_z;
		
		angle_y = T( asin( mat.rc(2,0) ) );
		if( angle_y < 1.5707963 && angle_y > -1.5707963 ) // cos > 0
		{
//			angle_x = T( atan( mat.rc(2,1) / mat.rc(2,2) ) );
//			angle_z = T( atan( mat.rc(1,0) / mat.rc(0,0) ) );
			angle_x = T( atan2( mat.rc(2,1), mat.rc(2,2) ) );
			angle_z = T( atan2( mat.rc(1,0), mat.rc(0,0) ) );
		}
		else
		{
			angle_x = T( 3.141592654 + atan2( mat.rc(2,1), mat.rc(2,2) ) );
			angle_z = T( 3.141592654 + atan2( mat.rc(1,0), mat.rc(0,0) ) );
//			angle_x = T( 3.141592654 + atan( mat.rc(2,1) / mat.rc(2,2) ) );
//			angle_z = T( 3.141592654 + atan( mat.rc(1,0) / mat.rc(0,0) ) );
		}
		return Vector3<T>( angle_x, angle_y, angle_z );
	}
	void split(Vector3<T> * t, Vector3<T> * r, Vector3<T> * s)
	{
		t->x = MatrixBase::rc(0,3);
		t->y = MatrixBase::rc(1,3);
		t->z = MatrixBase::rc(2,3);
		Matrix4<T> mat = *this;
		s->x = T( sqrt( mat.rc(0,0)*mat.rc(0,0) + mat.rc(0,1)*mat.rc(0,1) + mat.rc(0,2)*mat.rc(0,2) ) );
		s->y = T( sqrt( mat.rc(1,0)*mat.rc(1,0) + mat.rc(1,1)*mat.rc(1,1) + mat.rc(1,2)*mat.rc(1,2) ) );
		s->z = T( sqrt( mat.rc(2,0)*mat.rc(2,0) + mat.rc(2,1)*mat.rc(2,1) + mat.rc(2,2)*mat.rc(2,2) ) );
		mat.rc(0,0) /= s->x;
		mat.rc(0,1) /= s->x;
		mat.rc(0,2) /= s->x;
		mat.rc(1,0) /= s->y;
		mat.rc(1,1) /= s->y;
		mat.rc(1,2) /= s->y;
		mat.rc(2,0) /= s->z;
		mat.rc(2,1) /= s->z;
		mat.rc(2,2) /= s->z;
		*r = factorization( mat );
	}
	
	void nocopymult(const Matrix4 & m1, const Matrix4 & m2)
	{
		*this = m1 * m2;
	}
};


template<typename T>
Vector3<T> operator* ( const Matrix4<T> & m, const Vector3<T> & v )
{
	Vector3<T> vt;
	vt.x = v.x*m.rc(0,0) + v.y*m.rc(0,1) + v.z*m.rc(0,2) + m.rc(0,3);
	vt.y = v.x*m.rc(1,0) + v.y*m.rc(1,1) + v.z*m.rc(1,2) + m.rc(1,3);
	vt.z = v.x*m.rc(2,0) + v.y*m.rc(2,1) + v.z*m.rc(2,2) + m.rc(2,3);
	T w = v.x*m.rc(3,0) + v.y*m.rc(3,1) + v.z*m.rc(3,2) + m.rc(3,3);
	vt /= w;
	return vt;
}


template <typename T>
Vector3<T> operator* ( const Vector3<T> & v, const Matrix4<T> & m )
{
	Vector3<T> vt;
	vt.x = v.x*m.rc(0,0) + v.y*m.rc(1,0) + v.z*m.rc(2,0) + m.rc(3,0);
	vt.y = v.x*m.rc(0,1) + v.y*m.rc(1,1) + v.z*m.rc(2,1) + m.rc(3,1);
	vt.z = v.x*m.rc(0,2) + v.y*m.rc(1,2) + v.z*m.rc(2,2) + m.rc(3,2);
//		T  w = v.x*m.x(3,0) + v.y*m.x(3,1) + v.z*m.x(3,2) + m.x(3,3);
//		vt /= w;
	return vt;
}

#ifdef __APPLE__
#if (TARGET_IPHONE_SIMULATOR == 0) && (TARGET_OS_IPHONE == 1)

template<>
class Matrix4<float>: public MatrixSquare<float,4>
{
	typedef float T;
	typedef MatrixSquare<T,4> MatrixBase;
	public:
	Matrix4()
	{}
	Matrix4( const MatrixSquare<T,4> & mat )
	:	MatrixSquare<T,4>( mat )
	{}

	static Matrix4 translate( const Vector3<T> & v )
	{
		Matrix4 tra = MatrixBase::identity();
		tra.rc(0,3) = v.x;
		tra.rc(1,3) = v.y;
		tra.rc(2,3) = v.z;
		return tra;
		}
		static Matrix4 scale( const Vector3<T> & v )
		{
			Matrix4 sca = MatrixBase::identity();
			sca.rc(0,0) = v.x;
			sca.rc(1,1) = v.y;
			sca.rc(2,2) = v.z;
			return sca;
		}
		static Matrix4 invert( Matrix4 m )
		{
			Matrix4 res = MatrixBase::identity();
			for( int i = 0; i < 4; ++i )
			{
				int ma = i;
				for( int j = ma + 1; j < 4; ++j )
					if( fabs( m.rc(j, i) ) > fabs( m.rc(ma, i) ) )
						ma = j;
				if( ma != i )
					for( int nx = 0; nx < 4; ++nx )
					{
						T tmp = m.rc( i, nx );
						m.rc( i, nx ) = m.rc( ma, nx );
						m.rc( ma, nx ) = tmp;
						tmp = res.rc( i, nx );
						res.rc( i, nx ) = res.rc( ma, nx );
						res.rc( ma, nx ) = tmp;
					}
				for( int ny = 0; ny < 4; ++ny )
					if( ny == i )
					{
						T mul = 1 / m.rc(i, i);
						for( int nx = 0; nx < 4; ++nx )
						{
							m.rc( ny, nx ) *= mul;
							res.rc( ny, nx ) *= mul;
						}
					}
					else
					{
						T mul = m.rc( ny, i ) / m.rc(i, i);
						for( int nx = 0; nx < 4; ++nx )
						{
							m.rc( ny, nx ) -= mul * m.rc( i, nx );
							res.rc( ny, nx ) -= mul * res.rc( i, nx );
						}
					}
			}
			return res;
		}
		Vector3<T> shift()const
		{
			return Vector3<T>( MatrixBase::rc(0,3), MatrixBase::rc(1,3), MatrixBase::rc(2,3) );
		}
		
		static Matrix4 rotate( const Vector3<T> & angles )
		{
			T cz(cos( angles.z ));
			T sz(sin( angles.z ));
			T cy(cos( angles.y ));
			T sy(sin( angles.y ));
			T cx(cos( angles.x ));
			T sx(sin( angles.x ));
			Matrix4 mat = MatrixBase::identity();
			mat.rc(0,0) = cz*cy;
			mat.rc(1,0) = sz*cy;
			mat.rc(2,0) = sy;
			mat.rc(0,1) = -cz*sx*sy - sz*cx;
			mat.rc(1,1) = -sz*sx*sy + cz*cx;
			mat.rc(2,1) = sx*cy;
			mat.rc(0,2) = -cz*cx*sy + sz*sx;
			mat.rc(1,2) = -sz*cx*sy - cz*sx;
			mat.rc(2,2) = cx*cy;
			return mat;
		}
		
		static Matrix4 rotate( const Vector3<T> & axe, T angle )
		{
			Matrix4 mat = MatrixBase::identity();
			
			T c = (T)cos( angle );
			T s = (T)sin( angle );
			
			mat.rc(0,0) = axe.x*axe.x		+(1-axe.x*axe.x)*c;
			mat.rc(0,1) = axe.x*axe.y*(1-c)	+axe.z*s;
			mat.rc(0,2) = axe.x*axe.z*(1-c)	-axe.y*s;
			
			mat.rc(1,0) = axe.x*axe.y*(1-c)	-axe.z*s;
			mat.rc(1,1) = axe.y*axe.y			+(1-axe.y*axe.y)*c;
			mat.rc(1,2) = axe.y*axe.z*(1-c)	+axe.x*s;
			
			mat.rc(2,0) = axe.x*axe.z*(1-c)	+axe.y*s;
			mat.rc(2,1) = axe.y*axe.z*(1-c)	-axe.x*s;
			mat.rc(2,2) = axe.z*axe.z			+(1-axe.z*axe.z)*c;
			return mat;
		}
		
		static Vector3<T> factorization( const Matrix4 & mat )
		{
			T angle_x, angle_y, angle_z;
			
			angle_y = T( asin( mat.rc(2,0) ) );
			if( angle_y < 1.5707963 && angle_y > -1.5707963 ) // cos > 0
			{
				//			angle_x = T( atan( mat.rc(2,1) / mat.rc(2,2) ) );
				//			angle_z = T( atan( mat.rc(1,0) / mat.rc(0,0) ) );
				angle_x = T( atan2( mat.rc(2,1), mat.rc(2,2) ) );
				angle_z = T( atan2( mat.rc(1,0), mat.rc(0,0) ) );
			}
			else
			{
				angle_x = T( 3.141592654 + atan2( mat.rc(2,1), mat.rc(2,2) ) );
				angle_z = T( 3.141592654 + atan2( mat.rc(1,0), mat.rc(0,0) ) );
				//			angle_x = T( 3.141592654 + atan( mat.rc(2,1) / mat.rc(2,2) ) );
				//			angle_z = T( 3.141592654 + atan( mat.rc(1,0) / mat.rc(0,0) ) );
			}
			return Vector3<T>( angle_x, angle_y, angle_z );
		}
		void split(Vector3<T> * t, Vector3<T> * r, Vector3<T> * s)
		{
			t->x = MatrixBase::rc(0,3);
			t->y = MatrixBase::rc(1,3);
			t->z = MatrixBase::rc(2,3);
			Matrix4<T> mat = *this;
			s->x = T( sqrt( mat.rc(0,0)*mat.rc(0,0) + mat.rc(0,1)*mat.rc(0,1) + mat.rc(0,2)*mat.rc(0,2) ) );
			s->y = T( sqrt( mat.rc(1,0)*mat.rc(1,0) + mat.rc(1,1)*mat.rc(1,1) + mat.rc(1,2)*mat.rc(1,2) ) );
			s->z = T( sqrt( mat.rc(2,0)*mat.rc(2,0) + mat.rc(2,1)*mat.rc(2,1) + mat.rc(2,2)*mat.rc(2,2) ) );
			mat.rc(0,0) /= s->x;
			mat.rc(0,1) /= s->x;
			mat.rc(0,2) /= s->x;
			mat.rc(1,0) /= s->y;
			mat.rc(1,1) /= s->y;
			mat.rc(1,2) /= s->y;
			mat.rc(2,0) /= s->z;
			mat.rc(2,1) /= s->z;
			mat.rc(2,2) /= s->z;
			*r = factorization( mat );
		}
	
		const Matrix4 & operator*= ( const Matrix4 & m )
		{
			Matrix4Mul(data(), m.data(), data());
			return *this;
		}
	
		void nocopymult(const Matrix4 & m1, const Matrix4 & m2)
		{
			Matrix4Mul(m1.data(), m2.data(), data());
		}
	};
	
	
	inline Vector3<float> operator* ( const Matrix4<float> & m, const Vector3<float> & v )
	{
		float res[4];
		Matrix4Vector3Mul(m.data(), v.data(), res);
		return Vector3<float>(res[0], res[1], res[2]);
		
	}
	
	
	inline Vector3<float> operator* ( const Vector3<float> & v, const Matrix4<float> & m )
	{
		float res[4];
		Vector3Matrix4Mul(v.data(), m.data(), res);
		return Vector3<float>(res[0], res[1], res[2]);
	}

	inline Matrix4<float> operator* ( const Matrix4<float> & m1, const Matrix4<float> & m2 )
	{
		Matrix4<float> res;
		Matrix4Mul(m1.data(), m2.data(), res.data());
		return res;
	}
	
#endif
#endif
	
} // namespace math
} // namespace liba
using namespace liba;




#endif //__Matrix4_H__

